#!/usr/bin/python
import roslib; roslib.load_manifest('DelphinROSv2')
import rospy
import numpy
import math
import time

#import message types for publishing:
from std_msgs.msg import Float32
from std_msgs.msg import Bool
from std_msgs.msg import Int8

#import message types for subscribing:
from DelphinROSv2.msg import compass
from DelphinROSv2.msg import position
from DelphinROSv2.msg import tail_setpoints
from DelphinROSv2.msg import tsl_setpoints
from DelphinROSv2.msg import altitude
from DelphinROSv2.msg import tsl_feedback
from DelphinROSv2.msg import status

class library_highlevel:
# library class for high-level controller.  Defines the inputs, output and basic functionality that
# all high-level controllers should inherit.
	

    def __init__(self):
        #constructor method - creates objects of type 'HighLevelController'
        
        #set up publishers (stored as object variables)
        self.pub_sway_demand            = rospy.Publisher('sway_demand', Float32)
        self.pub_prop_demand            = rospy.Publisher('prop_demand',Int8) #tail_section.py currently expecting uint8
        self.pub_heading_demand         = rospy.Publisher('heading_demand', Float32) 
        self.pub_depth_demand           = rospy.Publisher('depth_demand', Float32) 
        self.pub_tsl_onOff_horizontal   = rospy.Publisher('TSL_onOff_horizontal', Bool) #turns thrusters on and off 
        self.pub_tsl_onOff_vertical     = rospy.Publisher('TSL_onOff_vertical', Bool) #turns thrusters on and off
        self.pub_tail_setpoints         = rospy.Publisher('tail_setpoints', tail_setpoints)
        self.pub_tsl_heading            = rospy.Publisher('TSL_setpoints_horizontal', tsl_setpoints)
        self.pub_tsl_depth              = rospy.Publisher('TSL_setpoints_vertical', tsl_setpoints)
        self.pub_heading_control_onOff  = rospy.Publisher('Heading_onOFF', Bool) #turns heading controller on and off
        self.pub_depth_control_onOff    = rospy.Publisher('Depth_onOFF', Bool) #turns depth controller on and off

        #set up subscribers
        rospy.Subscriber('compass_out', compass, self.callback_compass) #compass data
        rospy.Subscriber('position_dead', position, self.callback_position) #position (dead reckoning)
        rospy.Subscriber('altimeter_out', altitude, self.callback_altitude)
        rospy.Subscriber('back_seat_flag', Int8, self.callback_back_seat_flag)
        rospy.Subscriber('TSL_feedback', tsl_feedback, self.callback_voltage)
        rospy.Subscriber('status', status, self.callback_status)
        

        #gets parameters from server ####Don't Need to be Here ????????
        self.__maxDepthDemand = rospy.get_param('max-depth-demand')
        self.overdepth = rospy.get_param('over-depth')
        self.overpitch = rospy.get_param('over-pitch')
        
	    
        #initilise empty object variables to hold latest data from subscribers
        ######### might need to set default values in message files
        self.__compass = compass()
        self.__position = position()
        self.__altitude = altitude()
        self.__back_seat_flag = 0 #back_seat_flag is 0 when no errors are present
        self.__motor_voltage = 0
        
        self.__TSL_status = 0 #initisliased to indicate presence of errors - this will be set to 1 once node has been tested during vehicle initialisation
        self.__tail_status = 0
        self.__altimeter_status = 0
        self.__gps_status = 0
        self.__compass_status = 0
   
    # stops vehicle and shuts down ROS
    # may be better as an action....
    def stop(self):
        rospy.logfatal("Stop method invoked - ROS will shut down in 5 seconds")
        self.setTSLHorizontal(0, 0)
        self.switchHorizontalThrusters(0)
        self.setTSLVertical(0, 0)
        self.switchVerticalThrusters(0)
        self.setRearProp(0)
        time.sleep(5)
        rospy.logfatal("Shutting down")
        rospy.signal_shutdown('mission finished')
    
    # sets sway 'demand' (tsl_setpoints ~500-1000)
    def sway(self, demand):
	str = "Swaying %s (positive to starboard)" %demand
        rospy.loginfo(str)
        # keep current heading (not current heading demand!)
        #self.changeHeadingBy(0)
        self.pub_sway_demand.publish(demand)
    
    # sets a 'demand' for the rear prop (between 0 and 22ish)
    def setRearProp(self, demand):
        #publish rear prop_demand
	print 'Rear prop to', demand
        str = "Setting rear prop demand %s" %demand
        rospy.loginfo(str)
        self.pub_prop_demand.publish(demand)
        
    # set a 'demand' (in degrees) for the rudder angle
    def setRudderAngle(self, demand):
	output=tail_setpoints()
        output.b=demand
        output.d=demand
        output.c=0
        output.e=0
	str = "Ruddder demand %s deg" %demand
        rospy.loginfo(str)
        #publish rear rudder_demand
        self.pub_tail_setpoints.publish(output)

    # set a 'demand' (in degrees) for the rudder angle
    def setControlSurfaceAngle(self, b,c,d,e):
	output=tail_setpoints()
        output.b=b
        output.d=d
        output.c=c
        output.e=e
        str = "Control surface demands - top: %s, sb: %s, bottom: %s, p: %s deg" %(b,c,d,e)
        rospy.loginfo(str)
        self.pub_tail_setpoints.publish(output)


    # manually send tsl setpoint values for horizontal thrusters
    def setTSLHorizontal(self, thruster2, thruster3):
	output=tsl_setpoints()
        output.thruster0=thruster2
        output.thruster1=thruster3
        str = "Horizontal Thruster Setpoints %s %s" %(thruster2,thruster3)
        rospy.loginfo(str)
        self.switchHorizontalThrusters(1)
        self.switchHeadingOnOff(0)
        time.sleep(0.5)
        self.pub_tsl_heading.publish(output)

    # manually send tsl setpoint values for horizontal thrusters
    def setTSLVertical(self, thruster0, thruster1):
	output=tsl_setpoints()
        output.thruster0=thruster0
        output.thruster1=thruster1
	#print statement might not work! hasn't been tested!
        str = "Vertical Thruster Setpoints %s %s" %(thruster0,thruster1)
        rospy.loginfo(str)
        self.switchVerticalThrusters(1)
        self.switchDepthOnOff(0)
        time.sleep(0.5)
        self.pub_tsl_depth.publish(output)


    # move to depth 'demand' (metres)
    def setDepth(self, demand):
        #publish depthDemand
        #print 'Setting depth demand: ', demand, 'm'
        self.switchDepthOnOff(1)
	self.switchVerticalThrusters(1)
        print demand
	if (demand < self.__maxDepthDemand and demand >0):
        	self.pub_depth_demand.publish(demand)
                str = "Setting depth demand %sm" %demand
                rospy.loginfo(str)
        elif demand <=0:
                str = "Requested depth demand %sm <=0m, turning vertical thrusters Off. " %demand
                rospy.logwarn(str)   
                self.switchDepthOnOff(0)
                self.switchVerticalThrusters(0)           
	else:
		self.pub_depth_demand.publish(self.__maxDepthDemand)
                str = "Requested depth %sm > maxDepthDemand (%sm)" %(demand, self.__maxDepthDemand)
                rospy.logwarn(str)
                str = "Setting depth demand %sm" %self.__maxDepthDemand
                rospy.logwarn(str)

    # move to heading 'demand' (degrees)
    def setHeading(self, demand):
        #publish headingDemand
	str = "Setting heading demand %s deg" %demand
        rospy.loginfo(str)
        self.switchHorizontalThrusters(1)
        self.switchHeadingOnOff(1)
        self.pub_heading_demand.publish(demand)

    # change heading by 'headingChange' (degrees)
    def changeHeadingBy(self, headingChange):
        #change heading by an amount (headingChange)
        self.setHeading(self.__compass.heading + headingChange)

    # change depth by 'depthChange' (metres)
    def changeDepthBy(self, depthChange):
        #change depth by an amount (depthChange)
        self.setDepth(self.__compass.depth + depthChange)
    
    # switch horizontal thrusters on or off {1,0}
    def switchHorizontalThrusters(self, onOff):
        if onOff == 1:
            self.pub_tsl_onOff_horizontal.publish(1)
            str = "Switch Horizontal Thruster ON"
            rospy.logdebug(str)	
        else: 
            self.pub_tsl_onOff_horizontal.publish(0) 
            str = "Switch Horizontal Thruster OFF"
            rospy.logdebug(str)	
            
    # switch vertical thrusters on or off {1,0}
    def switchVerticalThrusters(self, onOff):
        if onOff == 1:
            self.pub_tsl_onOff_vertical.publish(1) 
            str = "Switch Vertical Thruster ON"
            rospy.logdebug(str)	        
        else:
            self.pub_tsl_onOff_vertical.publish(0) 
	    str = "Switch Vertical Thruster OFF"
            rospy.logdebug(str)	 

    # switch heading controller on or off {1,0}
    def switchHeadingOnOff(self,onOff):
	if onOff ==1:
	    self.pub_heading_control_onOff.publish(1)
            str = "Switch Heading Control ON"
            rospy.logdebug(str)	 
        else:
            self.pub_heading_control_onOff.publish(0)
            str = "Switch Heading Control OFF"
            rospy.logdebug(str)	 

    # switch depth controller on or off {1,0}
    def switchDepthOnOff(self,onOff):
	if onOff ==1:
	    self.pub_depth_control_onOff.publish(1)
            str = "Switch Depth Control ON"
            rospy.logdebug(str)
        else:
            self.pub_depth_control_onOff.publish(0)
            str = "Switch Depth Control OFF"
            rospy.logdebug(str)
            
        
    #################################################
    # Getter methods    
    ############# these might change depending on which compass is being used...
    ############# could I create PNI compass class? etc that could be instantiated in constructor of this class?
    ############# or is that getting too complicated...?    
    def getHeading(self):
        return self.__compass.heading
    
    def getRoll(self):
        return self.__compass.roll
    
    def getPitch(self):
        return self.__compass.pitch
    
    def getTemperature(self):
        return self.__compass.temperature
    
    def getDepth(self):
        return self.__compass.depth
    
    # magnetometer
    def getM(self):
        return self.__compass.m
    
    def getMx(self):
        return self.__compass.mx
    
    def getMy(self):
        return self.__compass.my
    
    def getMz(self):
        return self.__compass.mz
    
    # accelerometer
    def getA(self):
        return self.__compass.a
    
    def getAx(self):
        return self.__compass.ax
    
    def getAy(self):
        return self.__compass.ay
    
    def getAz(self):
        return self.__compass.az

    # get position values
    def getX(self):
        return self.__position.X

    def getY(self):
        return self.__position.Y
               
    def getAltitude(self):
        return self.__altitude.altitude
    
    def getBackSeatErrorFlag(self):
        return self.__back_seat_flag
    
    def getVoltage(self):
        return self.__motor_voltage
    
    def getTSLStatus(self):
        return self.__TSL_status
    
    def getTailStatus(self):
        return self.__tail_status
        
    def getAltimeterStatus(self):
        return self.__altimeter_status
    
    def getGPSStatus(self):
        return self.__gps_status
    
    def getCompassStatus(self):
        return self.__compass_status
 
    #################################################
    # Callbacks
    #################################################
    def callback_compass(self, compass_data):
           if compass_data.depth > 15:
                str = "Depth exceeded 15m - aborting mission"
                rospy.logfatal(str)
                self.stop()
           self.__compass = compass_data
    
    def callback_position(self, position):
           self.__position = position   

    def callback_altitude(self, altitude):
           self.__altitude = altitude
           
    def callback_back_seat_flag(self, back_seat_flag):
           self.__back_seat_flag = back_seat_flag
           
    def callback_voltage(self, voltage):
           self.__motor_voltage = voltage.voltage
           
    def callback_status(self, status):
            if status.nodeID == 1:
                #TSL_board:
                self.__TSL_status = status.status
                return
            elif status.nodeID == 2:
                self.__tail_status = status.status
                return
            elif status.nodeID == 3:
                self.__altimeter_status = status.status
                return
            elif status.nodeID == 4:
                self.__gps_status = status.status
                return
            elif status.nodeID == 5:
                self.__compass_status = status.status
                return
